#ifndef __OBSERVATION_BUFFER_H__
#define __OBSERVATION_BUFFER_H__

#include <geometry_msgs/Point.h>
#include <sensor_msgs/PointCloud2.h>
#include <tf2_ros/buffer.h>
#include <boost/thread.hpp>
using namespace std;
using namespace tf2;

namespace costmap_2d{

class Observation{
    public:
    Observation():cloud_(new sensor_msgs::PointCloud2()), 
        obstacle_range_(0.0), 
        raytrace_range_(0.0){}
    
        virtual ~Observation(){
            delete cloud_;
        }

        Observation(geometry_msgs::Point& origin, const sensor_msgs::PointCloud2 &cloud,
            double obstacle_range, double raytrace_range) :
            origin_(origin), cloud_(new sensor_msgs::PointCloud2(cloud)),
            obstacle_range_(obstacle_range), raytrace_range_(raytrace_range){}

        Observation(const Observation& obs) :
            origin_(obs.origin_), cloud_(new sensor_msgs::PointCloud2(*(obs.cloud_))),
            obstacle_range_(obs.obstacle_range_), raytrace_range_(obs.raytrace_range_){}

        Observation(const sensor_msgs::PointCloud2 &cloud, double obstacle_range) :
            cloud_(new sensor_msgs::PointCloud2(cloud)), obstacle_range_(obstacle_range), raytrace_range_(0.0){}

        geometry_msgs::Point origin_;
        sensor_msgs::PointCloud2* cloud_;
        double obstacle_range_, raytrace_range_;        
};

class ObservationBuffer{
    public:
        ObservationBuffer(std::string topic_name, double observation_keep_time, 
            double obstacle_range, double raytrace_range, tf2_ros::Buffer& tf2_buffer, 
            std::string global_frame, double tf_tolerance);
        
        ~ObservationBuffer();

        void bufferCloud(const sensor_msgs::PointCloud2& cloud);

        void getObservations(std::vector<Observation>& observations);

        inline void lock(){
            lock_.lock();
        }

        inline void unlock(){
            lock_.unlock();
        }

    private:
        void purgeStaleObservations();
        tf2_ros::Buffer& tf2_buffer_;
        const ros::Duration observation_keep_time_;
        ros::Time last_updated_;
        std::list<Observation> observation_list_;
        boost::recursive_mutex lock_;
        double obstacle_range_, raytrace_range_;
        double tf_tolerance_;
        std::string global_frame_;
        std::string topic_name_;
};

}   // namespace costmap_2d


#endif